﻿// Decompiled with JetBrains decompiler
// Type: DnProcessSimulateCommands.ExportToRTR.CApExportToRTRRobot
// Assembly: DnProcessSimulateCommands, Version=2402.0.0.890, Culture=neutral, PublicKeyToken=null
// MVID: A885D881-AEDF-447E-852A-6F13102E089C
// Assembly location: C:\Program Files\Tecnomatix_2402\eMPower\DotNetCommands\DnProcessSimulateCommands.dll

using EngineeringInternalExtension;
using EngineeringInternalExtension.ModelObjects;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.IO;
using System.Linq;
using Tecnomatix.Engineering;


namespace DnProcessSimulateCommands.ExportToRTR{

public class CApExportToRTRRobot : CApExportToRTRExporter, ICApExportToRTRWriteable
{
  private TxRobot m_robot;
  private CApExportToRTRRobotSupplements m_supplements;
  private Dictionary<TxJoint, TxTransformation> m_jointsFrames = new Dictionary<TxJoint, TxTransformation>();
  private HashSet<TxKinematicLink> m_links = new HashSet<TxKinematicLink>();
  private int m_jointIndex;
  private TxReportWriterEx m_logWriter;

  public List<TxJoint> ExternalJoints { get; set; } = new List<TxJoint>();

  public CApExportToRTRRobot(
    TxRobot robot,
    string root,
    string worldLink,
    CApExportToRTRRobotSupplements supplements,
    TxReportWriterEx logWriter)
    : base(root, worldLink, ((TxBaseGeometryCreationObject) robot).Name)
  {
    this.m_logWriter = logWriter;
    this.m_robot = robot;
    this.m_supplements = supplements;
  }

  public void Write(CApExportToRTRXmlWriter writer)
  {
    this.m_jointIndex = 0;
    Directory.CreateDirectory(Path.Combine(this.m_root, this.m_directory));
    using (TxPoseData currentPose = this.m_robot.CurrentPose)
    {
      foreach (TxJoint drivingJoint in (Collection<ITxObject>) this.m_robot.DrivingJoints)
        drivingJoint.CurrentValue = 0.0;
      this.WriteBaseLink(writer);
      this.WriteDrivigJointsWithLinks(writer);
      this.WriteMountedTools(writer);
      this.WriteTCP(writer);
      this.m_robot.CurrentPose = currentPose;
    }
  }

  private void WriteDrivigJointsWithLinks(CApExportToRTRXmlWriter writer)
  {
    foreach (ITxObject drivingJoint in (Collection<ITxObject>) this.m_robot.DrivingJoints)
    {
      TxJoint joint = drivingJoint as TxJoint;
      if (TxJoint.op_Inequality(joint, (ITxObject) null))
      {
        this.WriteJointWithLink(joint, writer);
        ITxLocatableObject childLink = (ITxLocatableObject) joint.ChildLink;
        if (this.m_supplements.AttachedObjects.ContainsKey(childLink))
          this.WriteAttachmentsOfParent(joint, childLink, writer);
      }
    }
  }

  private void WriteJointWithLink(
    TxJoint joint,
    CApExportToRTRXmlWriter writer,
    bool isFixed = false,
    bool isExternalAxis = false)
  {
    CApExportToRTRJoint joint1 = this.CreateJoint(joint, isFixed, isExternalAxis);
    CApExportToRTRLink childLink = this.CreateChildLink(joint, joint1.GetChildLinkName());
    joint1.Write(writer);
    childLink?.Write(writer);
  }

  private void WriteAttachmentsOfParent(
    TxJoint joint,
    ITxLocatableObject parent,
    CApExportToRTRXmlWriter writer)
  {
    foreach (ITxLocatableObject itxLocatableObject in this.m_supplements.AttachedObjects[parent])
    {
      this.WriteAttachedObject(joint, parent, itxLocatableObject, writer);
      if (this.m_supplements.AttachedObjects.ContainsKey(itxLocatableObject))
        this.WriteAttachmentsOfParent(joint, itxLocatableObject, writer);
    }
  }

  private void WriteAttachedObject(
    TxJoint joint,
    ITxLocatableObject parent,
    ITxLocatableObject attachedObject,
    CApExportToRTRXmlWriter writer)
  {
    CApExportToRTRLink link = this.CreateLink((ITxObject) attachedObject, jointFrame: this.m_jointsFrames[joint]);
    new CApExportToRTRJoint(new CApExportToRTRJointData(this.GetItemName(((ITxObject) parent).Name), link.Name)).Write(writer);
    link.Write(writer);
  }

  private void WriteBaseLink(CApExportToRTRXmlWriter writer)
  {
    TxKinematicLink baseLink = this.GetBaseLink();
    if (!TxKinematicLink.op_Inequality(baseLink, (ITxObject) null))
      return;
    CApExportToRTRJoint exportToRtrJoint = new CApExportToRTRJoint(new CApExportToRTRJointData(this.m_parentLink, this.GetItemName(baseLink.Name)));
    CApExportToRTRLink robotLink = this.CreateRobotLink(baseLink);
    exportToRtrJoint.Write(writer);
    robotLink.Write(writer);
  }

  private void WriteMountedTools(CApExportToRTRXmlWriter writer)
  {
    foreach (ITxObject itxObject in this.m_supplements.MountedObjects.Objects)
      this.WriteTool(itxObject, writer);
    foreach (ITxDevice device in this.m_supplements.MountedDevices.Objects)
    {
      if (this.IsToolExternalAxis(device))
        this.WriteToolAsExternalAxis(device, writer);
      else
        this.WriteTool((ITxObject) device, writer);
    }
  }

  private void WriteToolAsExternalAxis(ITxDevice device, CApExportToRTRXmlWriter writer)
  {
    using (TxPoseData currentPose = device.CurrentPose)
    {
      foreach (TxJoint drivingJoint in (Collection<ITxObject>) device.DrivingJoints)
        drivingJoint.CurrentValue = 0.0;
      List<ITxObject> all = ((IEnumerable<ITxObject>) this.m_robot.ExternalAxes).ToList<ITxObject>().FindAll((Predicate<ITxObject>) (x => (x as TxJoint).Device == device));
      all.ForEach((Action<ITxObject>) (x => (x as TxJoint).ChildLink.Blank()));
      this.WriteTool((ITxObject) device, writer);
      foreach (ITxObject drivingJoint in (Collection<ITxObject>) device.DrivingJoints)
      {
        if (all.Contains(drivingJoint))
        {
          TxJoint joint = drivingJoint as TxJoint;
          joint.ChildLink.Display();
          this.WriteJointWithLink(joint, writer, isExternalAxis: true);
          ITxLocatableObject childLink = (ITxLocatableObject) joint.ChildLink;
          if (this.m_supplements.AttachedObjects.ContainsKey(childLink))
            this.WriteAttachmentsOfParent(joint, childLink, writer);
          this.ExternalJoints.Add(joint);
        }
      }
      device.CurrentPose = currentPose;
    }
  }

  private bool IsToolExternalAxis(ITxDevice device)
  {
    return device is TxServoGun & (this.m_robot.ExternalAxes != null && ((IEnumerable<ITxObject>) this.m_robot.ExternalAxes).ToList<ITxObject>().Exists((Predicate<ITxObject>) (x => (x as TxJoint).Device == device))) && this.IsGunSupported(device);
  }

  private bool IsGunSupported(ITxDevice device)
  {
    return !((IEnumerable<ITxObject>) device.AllJointsAfterCompilation).ToList<ITxObject>().Exists((Predicate<ITxObject>) (x => (x as TxJoint).IsDependent || TxJointEx.IsFollowing(x as TxJoint)));
  }

  private void WriteTool(ITxObject obj, CApExportToRTRXmlWriter writer)
  {
    TxJoint lastJoint = this.GetLastJoint();
    CApExportToRTRLink link = this.CreateLink(obj, jointFrame: this.m_jointsFrames[lastJoint]);
    new CApExportToRTRJoint(new CApExportToRTRJointData(this.GetItemName(lastJoint.ChildLink.Name), link.Name)).Write(writer);
    link.Write(writer);
    if (!(obj is ITxLocatableObject itxLocatableObject) || !this.m_supplements.AttachedObjects.ContainsKey(itxLocatableObject))
      return;
    this.WriteAttachmentsOfParent(lastJoint, itxLocatableObject, writer);
  }

  private void WriteTCP(CApExportToRTRXmlWriter writer)
  {
    string itemName = this.GetItemName("TCP");
    CApExportToRTRLink capExportToRtrLink = new CApExportToRTRLink(itemName);
    TxTransformation location = CApExportToRTRExportManager.UseWorkingFrame ? this.m_robot.TCPF.AbsoluteLocation.LocationRelativeToWorkingFrame : this.m_robot.TCPF.AbsoluteLocation;
    this.CreateTCPJoint(itemName, location).Write(writer);
    capExportToRtrLink.Write(writer);
  }

  private TxKinematicLink GetBaseLink()
  {
    foreach (TxKinematicLink link in (Collection<ITxObject>) this.m_robot.Links)
    {
      if (TxJoint.op_Equality(link.ParentJoint, (ITxObject) null))
        return link;
    }
    return (TxKinematicLink) null;
  }

  private TxJoint GetLastJoint() => CApExportToRTRUtils.GetRobotLastJoint(this.m_robot);

  private CApExportToRTRLink CreateChildLink(TxJoint joint, string linkName)
  {
    return this.CreateRobotLink(joint.ChildLink, linkName, this.m_jointsFrames[joint]);
  }

  private CApExportToRTRLink CreateRobotLink(
    TxKinematicLink kinematicLink,
    string linkName = null,
    TxTransformation jointFrame = null)
  {
    if (this.m_links.Contains(kinematicLink))
      return (CApExportToRTRLink) null;
    CApExportToRTRLink link = this.CreateLink((ITxObject) kinematicLink, linkName, jointFrame);
    this.m_links.Add(kinematicLink);
    return link;
  }

  private CApExportToRTRLink CreateLink(
    ITxObject kinematicLink,
    string linkName = null,
    TxTransformation jointFrame = null)
  {
    linkName = string.IsNullOrEmpty(linkName) ? kinematicLink.Name : linkName;
    CApExportToRTRLink link = new CApExportToRTRLink(this.GetItemName(linkName));
    string stlFileName = CApExportToRTRSTLWriter.GetStlFileName(kinematicLink);
    link.AddVisual(this.CreateVisual(stlFileName, jointFrame?.Inverse));
    this.CreateCadFile(kinematicLink, stlFileName);
    return link;
  }

  private CApExportToRTRJoint CreateTCPJoint(string TCPLinkName, TxTransformation location)
  {
    TxJoint lastJoint = this.GetLastJoint();
    string itemName = this.GetItemName(lastJoint.ChildLink.Name);
    string name = $"{itemName}_to_{TCPLinkName}";
    CApExportToRTROrigin origin = this.CreateOrigin(TxTransformation.op_Multiply(this.m_jointsFrames[lastJoint].Inverse, location));
    return new CApExportToRTRJoint(new CApExportToRTRJointData(name, JointType.FIXED, itemName, TCPLinkName, origin: origin));
  }

  private CApExportToRTRJoint CreateJoint(TxJoint joint, bool isFixed, bool isExternalAxis)
  {
    CApExportToRTRJoint joint1 = isExternalAxis ? (CApExportToRTRJoint) new CApExportToRTRExternalJoint(joint, this.m_robot) : new CApExportToRTRJoint(joint);
    string itemName1 = this.GetItemName(joint1.GetJointName());
    JointType type = isFixed ? JointType.FIXED : this.GetJointType(joint);
    string itemName2 = this.GetItemName(joint1.GetParentLinkName());
    string itemName3 = this.GetItemName(joint1.GetChildLinkName());
    TxTransformation jointFrame = this.GetJointFrame(joint);
    this.m_jointsFrames.Add(joint, jointFrame);
    double[] jointAxis = isFixed ? (double[]) null : this.GetJointAxis(joint);
    CApExportToRTROrigin jointOrigin = this.GetJointOrigin(joint, joint1.GetParentJoint());
    CApExportToRTRLimit limit = isFixed ? (CApExportToRTRLimit) null : new CApExportToRTRLimit(joint, 1.0, 100000.0);
    joint1.Data = new CApExportToRTRJointData(itemName1, type, itemName2, itemName3, this.m_jointIndex++, jointOrigin, jointAxis, limit: limit);
    return joint1;
  }

  private CApExportToRTROrigin GetJointOrigin(TxJoint joint, TxJoint parentJoint)
  {
    TxTransformation location = this.m_jointsFrames[joint];
    if (TxJoint.op_Inequality(parentJoint, (ITxObject) null))
      location = TxTransformation.op_Multiply(this.m_jointsFrames[parentJoint].Inverse, location);
    return this.CreateOrigin(location);
  }

  private TxTransformation GetJointFrame(TxJoint joint)
  {
    return new TxTransformation()
    {
      Translation = this.GetLocation(joint)
    };
  }

  private TxVector GetLocation(TxJoint joint)
  {
    TxVector jointDirection = this.GetJointDirection(joint);
    double num = Math.Sqrt(jointDirection.X * jointDirection.X + jointDirection.Y * jointDirection.Y + jointDirection.Z * jointDirection.Z) / 2.0;
    return TxVector.op_Addition(CApExportToRTRExportManager.UseWorkingFrame ? joint.Axis.FromPoint.LocationRelativeToWorkingFrame : joint.Axis.FromPoint, TxVector.op_Multiply(jointDirection, num));
  }

  private TxVector GetJointDirection(TxJoint joint)
  {
    TxVector jointDirection = TxVector.op_Subtraction(CApExportToRTRExportManager.UseWorkingFrame ? joint.Axis.ToPoint.LocationRelativeToWorkingFrame : joint.Axis.ToPoint, CApExportToRTRExportManager.UseWorkingFrame ? joint.Axis.FromPoint.LocationRelativeToWorkingFrame : joint.Axis.FromPoint);
    jointDirection.Normalize();
    return jointDirection;
  }

  private double[] GetJointAxis(TxJoint joint)
  {
    TxVector jointDirection = this.GetJointDirection(joint);
    TxVector txVector = this.m_jointsFrames[joint].TransformNormal(jointDirection);
    return new double[3]
    {
      txVector.X,
      txVector.Y,
      txVector.Z
    };
  }

  private JointType GetJointType(TxJoint joint)
  {
    return joint.Type == 2 ? JointType.PRISMATIC : JointType.REVOLUTE;
  }

  private string GetItemName(string name)
  {
    return $"{((TxBaseGeometryCreationObject) this.m_robot).Name}/{name}";
  }
}
